#include <iostream>
#include <math.h>
using namespace std; 

int main(){
	// 设矩阵保存到下面的矩阵中
	float m11,m12,m13;
	float m21,m22,m23;
	float m31,m32,m33;
	// 以弧度形式计算欧拉角并存在一下变量中
	float h,p,b;
	// 从m23计算pitch，小心asin()的域错误，因浮点计算我们允许一定的误差
	float sp = -m23;
	if(sp <= -1.0f){
		p = -1.570796f;	//-pi/2
	}else if(sp >= 1.0){
		p = 1.57076;	//pi/2
	}else{
		p = asin(sp);
	}
	// 检查万向锁的情况，允许一些误差
	if(sp > 0.9999f){
		// 用的是万向锁情况下p，h，b对用公式
		// 向正上或者正下看
		// 将bank置零，赋值给heading
		b = 0.0f;
		h = atan2(-m31,m11);
	}else{
		// 用的是平常情况下p，h，b对用公式
		// 通过m13和m33计算heading
		h = atan2(m13,m33);
		// 通过m21和m22计算bank
		b = atan2(m21,m22);
	}
	return 1;
}